#include "main.h"
#include "app_FIFO.h"

#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define MAX(a, b) ((a) < (b) ? (b) : (a))

#ifndef TRUE
#define TRUE  1
#endif
#ifndef FALSE
#define FALSE 0
#endif

#define SUCCESS				1
#define ERROR_NO_MEM		    0
#define ERROR_INVALID_LENGTH    		0


#define FIFO_LENGTH()           (p_fifo->Count)
#define IS_POWER_OF_TWO(SIZE)   is_power_of_2(SIZE)

/* 
判断数字是否是2的次幂
 */
#ifdef FIFI_2_POWER
static __INLINE uint8_t is_power_of_2(uint16_t n)
{
	return (n != 0 && ((n & (n - 1)) == 0));
}
#endif

/* 
计算队列的长度
 */
static __INLINE uint16_t fifo_length(FIFO_T * p_fifo)
{
    // uint16_t tmp = p_fifo->read_pos;
    // return p_fifo->write_pos - tmp;

    return p_fifo->Count;
}
/* 队列未使用的空间 */
static __INLINE uint16_t fifo_unused(FIFO_T *fifo)
{
#ifdef FIFI_2_POWER
	return (fifo->buf_size_mask + 1) - (fifo->write_pos - fifo->read_pos);
#else
    return fifo->write_pos - fifo->read_pos;
#endif
    
}


/**
 * @brief	写新数据到缓冲区
 * @param	p_fifo	: 指向队列的的实例
 * @param	m_pBuf: 队列指向的缓冲区
 * @param   buf_size: 队列的大小
 * @return	>0:成功 ; 0:失败
 */
int  FIFO_Init( FIFO_T* p_fifo ,uint8_t* m_pBuf ,uint16_t buf_size)
{
	if(!m_pBuf )return ERROR_NO_MEM;
	if(!buf_size)return ERROR_NO_MEM;

#ifdef FIFI_2_POWER
    // Check that the buffer size is a power of two.
    if (!IS_POWER_OF_TWO(buf_size))
    {
        return ERROR_INVALID_LENGTH;
    }

    /* 到这一步，buf_size_mask应该是除了最高bit位是0，其它全是1 */
    p_fifo->buf_size_mask = buf_size - 1;
#endif

	p_fifo->p_buf = m_pBuf;					/*缓冲区指针 */
	p_fifo->buf_size = buf_size;	

	p_fifo->read_pos = 0;
	p_fifo->write_pos = 0;			
	p_fifo->Count = 0;

    return SUCCESS;
}


//----------------static-------------------------------------------------------------
/**
 * @brief	查看指定位置的数据
 * @param	p_fifo	: 指向队列的的实例
 * @param	index: 队列的索引(下标)
 * @param   p_byte: 接收查看的数据指针
 */
static __INLINE void fifo_peek(FIFO_T * p_fifo, uint16_t index, uint8_t * p_byte)
{
#ifdef FIFI_2_POWER
    *p_byte = p_fifo->p_buf[(p_fifo->read_pos + index) & p_fifo->buf_size_mask];
#else
    
    *p_byte = p_fifo->p_buf[ index ];

#endif

}

/**
* @brief	从队列里取出一个数据，新读数据指针
* @param	p_fifo	: 指向队列的的实例
* @param   p_byte: 存放取出的数据
*/
static __INLINE void fifo_get(FIFO_T * p_fifo, uint8_t * p_byte)
{
//    fifo_peek(p_fifo, 0, p_byte);
	*p_byte = p_fifo->p_buf[ p_fifo->read_pos ];
	
    // p_fifo->read_pos++;
	p_fifo->read_pos = (++p_fifo->read_pos)%p_fifo->buf_size;
	p_fifo->Count -=1;

}


/**
* @brief	向队列里写入一个数据，新写数据指针
* @param	p_fifo	: 指向队列的的实例
* @param   byte: 要写入的数据
*/
static __INLINE void fifo_Input(FIFO_T * p_fifo, uint8_t byte)
{
#ifdef FIFI_2_POWER
    p_fifo->p_buf[p_fifo->write_pos & p_fifo->buf_size_mask] = byte;
#else
    p_fifo->p_buf[p_fifo->write_pos ] = byte;
#endif
    // p_fifo->write_pos++;
    p_fifo->write_pos = (++p_fifo->write_pos)%p_fifo->buf_size;
	// if(++p_fifo->write_pos >=p_fifo->buf_size)
	// {
	// 	p_fifo->write_pos =p_fifo->buf_size;
	// }
	p_fifo->Count+=1;
}

//====================================================================
/**
* @brief	如果队列还没满，就写入一个数据
* @param	p_fifo	: 指向队列的的实例
* @param   byte: 要写入的数据
* @return	>0:成功 ; =0:失败
* @remark  会更新写数据指针
*/
int app_fifo_Input(FIFO_T * p_fifo, uint8_t byte)
{
#ifdef FIFI_2_POWER
    if (FIFO_LENGTH() <= p_fifo->buf_size_mask)
#else
    if (FIFO_LENGTH() <= p_fifo->buf_size)
#endif
    {
        fifo_Input(p_fifo, byte);
        return SUCCESS;
    }

    return ERROR_NO_MEM;
}


/**
 * @brief	如果队列长度不为0，就取出一个数据
 * @param	p_fifo	: 指向队列的的实例
 * @param   p_byte: 接收查看的数据指针
 * @return	>0:成功 ; =0:失败
 * @remark  会更新读数据指针
 */
int app_fifo_get(FIFO_T * p_fifo, uint8_t * p_byte)
{
    if (FIFO_LENGTH() != 0)
    {
        fifo_get(p_fifo, p_byte);
        return SUCCESS;
    }

    return ERROR_NO_MEM;

}


/**
 * @brief	查看指定位置的数据
 * @param	p_fifo	: 指向队列的的实例
 * @param	index: 队列的索引(下标)
 * @param   p_byte: 接收查看的数据指针
 * @return	>0:成功 ; =0:失败
 * @remark  不会更新数据指针
 */
int app_fifo_peek(FIFO_T * p_fifo, uint16_t index, uint8_t * p_byte)
{
    if (FIFO_LENGTH() > index)
    {
        fifo_peek(p_fifo, index, p_byte);
        return SUCCESS;
    }

    return ERROR_NO_MEM;
}

//=============================================================

/**
 * @brief	写新数据到缓冲区
 * @param	p_fifo	: 指向队列的的实例
 * @param	m_pBuf: 队列指向的缓冲区
 * @param   p_size: 传入要读取的长度，返回读成功的长度，必须是变量(非常量)
 * @return	>0:成功 ; 0:失败
 */
int app_fifo_read(FIFO_T * p_fifo, uint8_t * p_byte_array, uint16_t * p_size)
{

    if(!p_fifo )return ERROR_NO_MEM;
    if(!p_size)return ERROR_NO_MEM;

    const uint16_t byte_count    = FIFO_LENGTH();//队列的长度
    const uint16_t requested_len = (*p_size);//取出的长度
    uint16_t       index         = 0;
    uint16_t       read_size     = MIN(requested_len, byte_count);//能取出的长度

    (*p_size) = byte_count;

    // Check if the FIFO is empty.
    // if (byte_count == 0)
    if (p_fifo->Count==0)
    {
        return ERROR_NO_MEM;
    }

    // Check if application has requested only the size.
    if (p_byte_array == NULL)
    {
        return ERROR_NO_MEM;
    }

    // Fetch bytes from the FIFO.
    while (index < read_size)
    {
        fifo_get(p_fifo, &p_byte_array[index++]);
    }

    (*p_size) = read_size;

    return SUCCESS;
}


/**
 * @brief	写新数据到缓冲区
 * @param	p_fifo	: 指向环形缓冲区的实例
 * @param	p_byte_array: 指向要写入环形缓冲区的数据地址
 * @param   p_size: 要写入前传入的数量,成功后写入的数量，必须是变量的地址
 * @return	1:成功写入; 0:写入失败 ;
 * @remark  此功能会更新队列,p_size返回成功写入的数量
 */
int app_fifo_write(FIFO_T * p_fifo, uint8_t const * p_byte_array, uint16_t * p_size)
{
    if(!p_fifo)return ERROR_NO_MEM;
    if(!p_size)return ERROR_NO_MEM;
#ifdef FIFI_2_POWER
    const uint16_t available_count = p_fifo->buf_size_mask - fifo_length(p_fifo) + 1;
#else
    const uint16_t available_count = p_fifo->buf_size - FIFO_LENGTH() ;
#endif
    const uint16_t requested_len   = (*p_size);
    uint16_t       index           = 0;
    uint16_t       write_size      = MIN(requested_len, available_count);

    (*p_size) = available_count;

    // Check if the FIFO is FULL.
    // if (available_count == 0)
    if (p_fifo->Count==p_fifo->buf_size)
    {
        return ERROR_NO_MEM;
    }

    // Check if application has requested only the size.
    if (p_byte_array == NULL)
    {
        return ERROR_NO_MEM;
    }

    //Fetch bytes from the FIFO.
    while (index < write_size)
    {
        fifo_Input(p_fifo, p_byte_array[index++]);
    }

    (*p_size) = write_size;

    return SUCCESS;
}
